#include "preprocess/pose_util.h"

namespace autodrive {
namespace perception {
Eigen::Affine3d pose_interpolate(std::int64_t const& timestamp,
                                 MatrixData& pre_rtk_data,
                                 MatrixData& current_rtk_data) noexcept(false) {
  auto preRTKtime{pre_rtk_data.timestamp};
  Eigen::Quaterniond prePoseRotation{pre_rtk_data.mat.block<3, 3>(0, 0)};
  Eigen::Vector3d prePoseTranslation{pre_rtk_data.mat.block<3, 1>(0, 3)};

  auto currentRTKtime{current_rtk_data.timestamp};
  Eigen::Quaterniond currentPoseRotation{
      current_rtk_data.mat.block<3, 3>(0, 0)};
  Eigen::Vector3d currentPoseTranslation{
      current_rtk_data.mat.block<3, 1>(0, 3)};

  if (currentRTKtime - preRTKtime == 0) {
    throw std::runtime_error("two matrix data has same timestamp");
  }

  auto const& tScaler = static_cast<double>(currentRTKtime - timestamp) /
                        static_cast<double>(currentRTKtime - preRTKtime);

  Eigen::Translation3d pose_trans;
  pose_trans.x() = prePoseTranslation.x() * tScaler +
                   currentPoseTranslation.x() * (1 - tScaler);
  pose_trans.y() = prePoseTranslation.y() * tScaler +
                   currentPoseTranslation.y() * (1 - tScaler);
  pose_trans.z() = prePoseTranslation.z() * tScaler +
                   currentPoseTranslation.z() * (1 - tScaler);
  Eigen::Quaterniond pose_rotation =
      prePoseRotation.slerp(1 - tScaler, currentPoseRotation);
  return pose_trans * pose_rotation;
}

Eigen::Affine3d gen_pose(
    std::int64_t const& timestamp,
    RTKDataBufferPtr const& rtkDataBufferPtr) noexcept(false) {
  if (rtkDataBufferPtr->GetNumFilled() == 0) {
    throw std::runtime_error(
        "not have ego motion data,not use ego motion pose.used timestamp is " +
        std::to_string(timestamp));
  }

  if (rtkDataBufferPtr->GetNumFilled() == 1) {
    throw std::runtime_error(
        "there is only 1 ego motion pose data,the only one ego motion "
        "timestamp "
        "is " +
        std::to_string(rtkDataBufferPtr->GetLatest().second->timestamp));
  }

  Eigen::Affine3d pose;
  bool bRet;
  std::string errMsg;

  auto const& latestRTKData{rtkDataBufferPtr->GetLatest()};
  auto const& latestRTKDataTimestamp{latestRTKData.second->timestamp};

  if (timestamp >= latestRTKDataTimestamp) {
    auto const& currIndex{latestRTKData.first};
    auto const& prevIndex{rtkDataBufferPtr->GetPrevIndex(currIndex)};

    auto const& currentRTKData{rtkDataBufferPtr->Get(currIndex)};
    auto const& prevRTKData{rtkDataBufferPtr->Get(prevIndex)};

    return pose_interpolate(timestamp, *prevRTKData, *currentRTKData);

  } else {
    auto tmpRtkTimestamp{latestRTKDataTimestamp};
    auto tmpIndex{latestRTKData.first};
    while (timestamp < tmpRtkTimestamp) {
      // std::cout << "[Info]----->compensatoposeMatrix:   "
      //           << "timestamp=" << timestamp
      //           << ", tmpRtkTimestamp=" << tmpRtkTimestamp << std::endl;
      if (rtkDataBufferPtr->HasPrev(tmpIndex)) {
        tmpIndex = rtkDataBufferPtr->GetPrevIndex(tmpIndex);
        tmpRtkTimestamp = rtkDataBufferPtr->Get(tmpIndex)->timestamp;
      } else {
        throw std::runtime_error(
            "no prev timestamp find in rtk data buffer,tmpRtkTimestamp=" +
            std::to_string(tmpRtkTimestamp) + ", rtk buffer length=" +
            std::to_string(rtkDataBufferPtr->GetNumFilled()));
      }
    }

    auto const& prevRTKdata{rtkDataBufferPtr->Get(tmpIndex)};
    auto const& nextIndex{rtkDataBufferPtr->GetNextIndex(tmpIndex)};
    auto const& nextRTKdata{rtkDataBufferPtr->Get(nextIndex)};

    return pose_interpolate(timestamp, *prevRTKdata, *nextRTKdata);
  }
}
}  // namespace perception
}  // namespace autodrive